feat(autoware_path_generator): function to smooth the path #227

sasakisasaki · 2025-02-26T13:41:02Z

Description

This is kind of feature porting from autoware.universe as follows

Import PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection from the autoware.universe side code
Also import all related functions from the autoware.universe side

Quality of Code

NOT ENOUGH YET. ~~I'm improving the code quality so this PR is ready for review.~~
~~The unit tests for the added feature has not been added yet.~~

The refactoring plan is mentioned in this issue.
Perhaps we can work on the refactoring with a few PRs. Including this idea for refactoring, please feel free to share your idea for the improvement of code quality. Thank you!

How was this PR tested?

I did vcs import at around 18:00 JST on 26th Feb. 2025
The executed commands are as follows:

$ git clone https://github.com/autowarefoundation/autoware.git
$ cd autoware

# Before the build
$ ./setup-dev-env.sh -y

(Enter password for user)

(ADDED: Perhaps you need to do `sudo shutdown -r now` for applying NVIDIA driver update)

$ vcs import src < autoware.repos

$ vim autoware-nightly.repos

(Edited `autoware.core` part so it used my branch on forked)

$ vcs import src < autoware-nightly.repos
$ source ~/autoware/install/setup.bash
$ sudo apt update && sudo apt upgrade
$ rosdep update
$ rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO

# Deploy the attached three files for the following place, respectively
# - src/launcher/autoware_launch/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/path_generator.param.yaml
# - src/launcher/autoware_launch/autoware_launch/launch/components/tier4_planning_component.launch.xml
# - src/universe/autoware.universe/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

behavior_planning_launch_xml.txt
tier4_planning_component_launch_xml.txt
path_generator_param_yaml.txt

# Now start colcon build
$ colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release

# Checked by psim
$ ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit

Screencast_psim.webm

Status of Additional Tests

(ADDED) It seems the tests on the evaluator shows a failed scenario link to failed one. I'm now investigating the issue.

Notes for reviewers

Please do not enable auto merge as this PR needed to be merged with this launch side PR at the same time.
Please feel free to provide all the needed tests for merging this PR. I'm really happy for performing the tests 👍

Effects on system behavior

Needed to be investigated during the testing process.

Description: This commit is kind of feature porting from `autoware.universe` as follows * Import `PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection` from the following `autoware.universe` code https://github.com/autowarefoundation/autoware.universe/blob/a0816b7e3e35fbe822fefbb9c9a8132365608b49/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp#L74-L104 * Also import all related functions from the `autoware.universe` side Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

github-actions · 2025-02-26T13:41:17Z

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

You've checked our contribution guidelines.
Your PR follows our pull request guidelines.
All required CI checks pass before marking the PR ready for review.

codecov · 2025-02-28T05:02:37Z

Codecov Report

Attention: Patch coverage is 63.02521% with 44 lines in your changes missing coverage. Please review.

Project coverage is 8.26%. Comparing base (4cb18f5) to head (d9b3a8d).
Report is 69 commits behind head on main.

Files with missing lines	Patch %	Lines
planning/autoware_path_generator/src/utils.cpp	61.22%	21 Missing and 17 partials ⚠️
planning/autoware_path_generator/src/node.cpp	71.42%	4 Missing and 2 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #227       +/-   ##
==========================================
- Coverage   78.75%   8.26%   -70.49%     
==========================================
  Files          11     122      +111     
  Lines         193   10196    +10003     
  Branches       73    1255     +1182     
==========================================
+ Hits          152     843      +691     
- Misses         11    9219     +9208     
- Partials       30     134      +104

Flag	Coverage Δ
differential	`8.26% <63.02%> (?)`
total	`?`

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

❄ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

planning/autoware_path_generator/src/utils.cpp

kosuke55 · 2025-02-28T08:25:52Z

planning/autoware_path_generator/src/utils.cpp

+  const double seg_dist =
+    autoware::motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1);


this is just calc_distance2d ?

Thank you for this informative comment. I also guessed this is just a calculation for 2-dimensional distance between two points. If this is not my misunderstanding, the calcSignedArcLength() function calculates the total path length by summing up the 2D distances between consecutive points, as shown in the following code:

Ref. to code:

https://github.com/autowarefoundation/autoware.core/blob/main/common/autoware_motion_utils/include/autoware/motion_utils/trajectory/trajectory.hpp#L714-L733

Perhaps we need to ensure which method should we use.

planning/autoware_path_generator/src/utils.cpp

planning/autoware_path_generator/include/autoware/path_generator/utils.hpp

planning/autoware_path_generator/src/utils.cpp

kosuke55 · 2025-02-28T08:53:17Z

planning/autoware_path_generator/include/autoware/path_generator/utils.hpp

+ * @param goal_lanelet Goal lanelet.
+ * @return True if the goal lanelet is found, false otherwise
+ */
+bool get_goal_lanelet(const PlannerData & planner_data, lanelet::ConstLanelet * goal_lanelet);


better not to use raw pointer

Fixed by this commit 👍

hmm not fixed?

Ahh, sorry for my bad! Let me fix soon!

planning/autoware_path_generator/src/utils.cpp

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp> Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

* Enhance error handlings * Remove unused variables * Simplify the code * Improve readability a little bit Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

…sasakisasaki/autoware.core into feat-embed-smooth-path-as-alpha-quality Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* This fix is for the following PR: autowarefoundation/autoware.core#227 Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* This comment is wrote because of my misunderstanding Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

sasakisasaki · 2025-03-04T11:25:38Z

Fixing error in some CI checks.

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* The goal position is generally separate from the path points Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* CI (especially unit tests) fails due to this sanity check * As this is out of scope for this PR, we will fix the bug where the start and end are reversed in another PR Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* We should start from the simple one * Then we can add the necessary optimization later Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

kosuke55 · 2025-03-08T09:16:18Z

planning/autoware_path_generator/config/path_generator.param.yaml

@@ -12,3 +12,5 @@
      search_distance: 30.0
      resampling_interval: 1.0
      angle_threshold_deg: 15.0
+    refine_goal_search_radius_range: 10.0  # [m]


the default value in behavior_path_planner is

refine_goal_search_radius_range: 7.5

do you have any reason chagagin from that?

Thanks for your feedback! Let me fix this to 7.5, as there is no specific reason. The reason was experimental use on the testing phase.

planning/autoware_path_generator/include/autoware/path_generator/common_structs.hpp

planning/autoware_path_generator/src/node.cpp

planning/autoware_path_generator/src/utils.cpp

kosuke55 · 2025-03-08T10:00:29Z

planning/autoware_path_generator/src/utils.cpp

+  // Replace the goal point with the refined goal
+  filtered_path.points.back().point.pose = goal;


⚠️

Is only the goal being replaced now?
Are the other points in the refine_goal_search_radius_range range be retained?
If so, I wonder if the goal and one of its previous points will have a larger deviation, causing a distortion of the path.

The original algorithm generates a smooth path by smoothly connecting between refine_goal_search_radius_range. After the points deletion, spline interpolation is executed in a later stage of the process to smooth them out.
please check original logic.
https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/#fixed_goal_planner

sorry I dit not give enough infomartion. 🙇

After the deletion, the splines are executed in a later stage of the process to smooth them out.

seems here: https://github.com/tier4/autoware.universe/blob/908cb7ee5cca01c367f03caf6db4562a620504fb/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp#L724-L725

so behavior path logic is

set refined goal and remove points whitiin radius in fixed_goal_planner side

spline interpolation is in behaivor_path_planner side

( this comment is by just reading code. I have not checked actual behavior in psim. I'm building now... )

Thank you for the detailed and kind lecture! It seems my misunderstanding is derived from as follows. Now I should be understanding the point. Let me fix this too!

The minimum distance range is assumed not from the current position (front of the path points), but from the goal

kosuke55 · 2025-03-08T10:12:44Z

I changed PR title route -> path. route is like missoin level output

kosuke55 · 2025-03-08T10:17:41Z

reopened #227 (comment)

kosuke55 · 2025-03-08T10:21:01Z

planning/autoware_path_generator/src/node.cpp

+
+  if (distance_to_goal < planner_data_.path_generator_parameters.refine_goal_search_radius_range) {


now this is a limitation:
cannnot generate paths to goals further than refine_goal_search_radius_range from the centerline.
I want to resolve this limitation in a later PR.

Ahhh, that is good observation. I did not notice such the wider road case.
In this PR, let us keep this specification the that limitation. Maybe we can use the width of the whole road to optimize the search radius range automatically.

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* The type of returned value and arguments were wrong Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

…or/common_structs.hpp Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp> Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp> Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

…sasakisasaki/autoware.core into feat-embed-smooth-path-as-alpha-quality Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

…-alpha-quality

…sasakisasaki/autoware.core into feat-embed-smooth-path-as-alpha-quality Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* autowarefoundation#227 (comment) Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

…sasakisasaki/autoware.core into feat-embed-smooth-path-as-alpha-quality Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

sasakisasaki · 2025-03-08T13:40:14Z

planning/autoware_path_generator/src/utils.cpp

+  path_up_to_just_before_pre_goal.points.push_back(prepare_pre_goal(goal, lanes));
+
+  // Replace the goal point with the refined goal
+  path_up_to_just_before_pre_goal.points.back().point.pose = goal;


Sorry, this is a bug. Let me fix soon.

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

sasakisasaki added run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) component:planning Route planning, decision-making, and navigation. (auto-assigned) labels Feb 26, 2025

sasakisasaki self-assigned this Feb 26, 2025

style(pre-commit): autofix

04596a1

xmfcx added the type:new-feature New functionalities or additions, feature requests. label Feb 28, 2025

sasakisasaki added 2 commits February 28, 2025 14:54

Merge branch 'main' into feat-embed-smooth-path-as-alpha-quality

f1f499a

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

bugs: fix remaining conflicts

17054c9

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

kosuke55 reviewed Feb 28, 2025

View reviewed changes

planning/autoware_path_generator/src/utils.cpp Outdated Show resolved Hide resolved

planning/autoware_path_generator/src/utils.cpp Outdated Show resolved Hide resolved

kosuke55 reviewed Feb 28, 2025

View reviewed changes

sasakisasaki and others added 7 commits February 28, 2025 18:11

Update planning/autoware_path_generator/src/utils.cpp

5a2766e

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp> Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

Update planning/autoware_path_generator/src/utils.cpp

808fb5d

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp> Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

refactor: as follows

19185eb

* Enhance error handlings * Remove unused variables * Simplify the code * Improve readability a little bit Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Merge branch 'feat-embed-smooth-path-as-alpha-quality' of github.com:…

fc90ae4

…sasakisasaki/autoware.core into feat-embed-smooth-path-as-alpha-quality Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

style(pre-commit): autofix

c2e91f9

refactor: enhance error handling

5945ef7

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

style(pre-commit): autofix

c4b3500

sasakisasaki added a commit to sasakisasaki/autoware_launch that referenced this pull request Mar 4, 2025

feat(path_generator): add parameters (see below)

e1fabc8

* This fix is for the following PR: autowarefoundation/autoware.core#227 Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

sasakisasaki mentioned this pull request Mar 4, 2025

feat(path_generator): add parameters autowarefoundation/autoware_launch#1343

Draft

sasakisasaki added 5 commits March 4, 2025 18:24

bug: fix wrong function declaration in header

ace5fbe

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

bug: fix wrong point index calculation

0dee050

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

bug: remove meaningless comment

b6a8492

* This comment is wrote because of my misunderstanding Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

fix: apply pre-commit

ea3bfed

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

fix: smooth path before cropping trajectory points

a322d4d

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

sasakisasaki mentioned this pull request Mar 4, 2025

Refactoring Plan for autoware_path_generator #236

Open

6 tasks

bug: fix shadow variable

865b38f

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

sasakisasaki and others added 7 commits March 5, 2025 19:54

Merge branch 'main' into feat-embed-smooth-path-as-alpha-quality

0a4c0db

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

fix: smooth path only when a goal point is included

2e286d9

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

bug: do error handling

8093a99

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

style(pre-commit): autofix

06c0532

bug: fix wrong distance calculation

9f217b3

* The goal position is generally separate from the path points Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

fix: remove sanity check temporary as following reasons

6509955

* CI (especially unit tests) fails due to this sanity check * As this is out of scope for this PR, we will fix the bug where the start and end are reversed in another PR Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

refactor: fix complexity

daef6de

* We should start from the simple one * Then we can add the necessary optimization later Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

kosuke55 reviewed Mar 8, 2025

View reviewed changes

kosuke55 changed the title ~~feat(autoware_path_generator): function to smooth the route~~ feat(autoware_path_generator): function to smooth the path Mar 8, 2025

kosuke55 reviewed Mar 8, 2025

View reviewed changes

kosuke55 self-assigned this Mar 8, 2025

sasakisasaki and others added 14 commits March 8, 2025 20:28

bug: missing fixes in the include header

764092b

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

bug: inconsistent function declaration

4688b8b

* The type of returned value and arguments were wrong Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Update planning/autoware_path_generator/include/autoware/path_generat…

576a002

…or/common_structs.hpp Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp> Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

Update planning/autoware_path_generator/src/node.cpp

56f36c7

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp> Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

Update planning/autoware_path_generator/src/utils.cpp

5872f9c

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp> Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

Update planning/autoware_path_generator/src/utils.cpp

468539c

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp> Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>

style(pre-commit): autofix

fb39145

Merge branch 'feat-embed-smooth-path-as-alpha-quality' of github.com:…

fa4522f

…sasakisasaki/autoware.core into feat-embed-smooth-path-as-alpha-quality Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Merge branch 'autowarefoundation:main' into feat-embed-smooth-path-as…

6b11019

…-alpha-quality

Merge branch 'feat-embed-smooth-path-as-alpha-quality' of github.com:…

933abd7

…sasakisasaki/autoware.core into feat-embed-smooth-path-as-alpha-quality Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

fix: apply comment in the following PR

7d64da2

* autowarefoundation#227 (comment) Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

fix: sorry, I was missing one comment to be applied

ed64aaf

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

style(pre-commit): autofix

d22762f

Merge branch 'feat-embed-smooth-path-as-alpha-quality' of github.com:…

23b2efc

…sasakisasaki/autoware.core into feat-embed-smooth-path-as-alpha-quality Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

sasakisasaki commented Mar 8, 2025

View reviewed changes

bug: fix wrong goal point interpolation

d9b3a8d

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat(autoware_path_generator): function to smooth the path #227

feat(autoware_path_generator): function to smooth the path #227

sasakisasaki commented Feb 26, 2025 •

edited

Loading

github-actions bot commented Feb 26, 2025 •

edited

Loading

codecov bot commented Feb 28, 2025 •

edited

Loading

kosuke55 Feb 28, 2025

sasakisasaki Feb 28, 2025

sasakisasaki Mar 1, 2025

kosuke55 Feb 28, 2025

sasakisasaki Mar 1, 2025

kosuke55 Mar 8, 2025

sasakisasaki Mar 8, 2025

sasakisasaki commented Mar 4, 2025

kosuke55 Mar 8, 2025

sasakisasaki Mar 8, 2025

kosuke55 Mar 8, 2025 •

edited

Loading

kosuke55 Mar 8, 2025 •

edited

Loading

kosuke55 Mar 8, 2025

sasakisasaki Mar 8, 2025

kosuke55 commented Mar 8, 2025

kosuke55 commented Mar 8, 2025

kosuke55 Mar 8, 2025 •

edited

Loading

sasakisasaki Mar 8, 2025

sasakisasaki Mar 8, 2025

		const double seg_dist =
		autoware::motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1);

		// Replace the goal point with the refined goal
		filtered_path.points.back().point.pose = goal;


		if (distance_to_goal < planner_data_.path_generator_parameters.refine_goal_search_radius_range) {

feat(autoware_path_generator): function to smooth the path #227

Are you sure you want to change the base?

feat(autoware_path_generator): function to smooth the path #227

Conversation

sasakisasaki commented Feb 26, 2025 • edited Loading

Description

Quality of Code

How was this PR tested?

Status of Additional Tests

Notes for reviewers

Effects on system behavior

github-actions bot commented Feb 26, 2025 • edited Loading

codecov bot commented Feb 28, 2025 • edited Loading

Codecov Report

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sasakisasaki commented Mar 4, 2025

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kosuke55 Mar 8, 2025 • edited Loading

Choose a reason for hiding this comment

kosuke55 Mar 8, 2025 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kosuke55 commented Mar 8, 2025

kosuke55 commented Mar 8, 2025

kosuke55 Mar 8, 2025 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sasakisasaki commented Feb 26, 2025 •

edited

Loading

github-actions bot commented Feb 26, 2025 •

edited

Loading

codecov bot commented Feb 28, 2025 •

edited

Loading

kosuke55 Mar 8, 2025 •

edited

Loading

kosuke55 Mar 8, 2025 •

edited

Loading

kosuke55 Mar 8, 2025 •

edited

Loading